﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Microsoft.Kinect;
using Kinect.Toolbox;

namespace DetectingKinectSencor
{
    public static class SkeletonTools
    {

        private static CoordinateMapper coordinateMapper;

        ////////////////////////////////////////////////
        //          Method Get Skeletons              //
        ////////////////////////////////////////////////

        /// <summary>
        /// Static method.
        /// This is a helper method and returns the tracked skeletons from a frame
        /// </summary>
        /// <param name="frame"></param>
        /// <returns></returns>
        public static Skeleton[] GetSkeletons(this SkeletonFrame frame)
        {
            if (frame == null)
                return null;

            // Create a new skeleton object from the length of the number of tracked skeletons
            var skeletons = new Skeleton[frame.SkeletonArrayLength];

            //Copies the content from the frame into the skeletons object
            frame.CopySkeletonDataTo(skeletons);

            //Return the skeleton objects
            return skeletons;
        }



        ////////////////////////////////////////////////
        //          Method Get Skeletons              //
        ////////////////////////////////////////////////

        /// <summary>
        /// Static helper method.
        /// Different to the GetSkeletons(frame) method is that here directy the reference to the skeleton is
        /// transferred as a paramater. Hence every update is directly made in the ref skeleton
        /// This method is used by the Draw method in SkeletonDisplayManager class, and does not create an 
        /// new array everytime -> better performance
        /// </summary>
        /// <param name="frame"></param>
        /// <param name="skeletons"></param>
        public static void GetSkeletons(SkeletonFrame frame, ref Skeleton[] skeletons)
        {
            if (frame == null)
                return;

            if (skeletons == null || skeletons.Length != frame.SkeletonArrayLength)
            {
                skeletons = new Skeleton[frame.SkeletonArrayLength];
            }

            //copy the data of the frame into the skeletons array
            frame.CopySkeletonDataTo(skeletons);
        }


        ////////////////////////////////////////////////
        //          Method Convert Vector2            //
        ////////////////////////////////////////////////

        /// <summary>
        /// This method converts joint coordinates from skeleton space (x,y,z in meter units) 
        /// to screen space (in pixel units).
        /// </summary>
        /// <param name="sensor"></param>
        /// <param name="position"></param>
        /// <returns></returns>
        public static Vector2 Convert(KinectSensor sensor, SkeletonPoint position)
        {
            float width = 0;
            float height = 0;
            float x = 0;
            float y = 0;

            if (sensor.ColorStream.IsEnabled)
            {
                coordinateMapper = new CoordinateMapper(sensor);

                var colorPoint = coordinateMapper.MapSkeletonPointToColorPoint(position, sensor.ColorStream.Format);

                x = colorPoint.X;
                y = colorPoint.Y;

                switch (sensor.ColorStream.Format)
                {
                    case ColorImageFormat.RawYuvResolution640x480Fps15:
                    case ColorImageFormat.RgbResolution640x480Fps30:
                    case ColorImageFormat.YuvResolution640x480Fps15:
                        width = 640;
                        height = 480;
                        break;
                    case ColorImageFormat.RgbResolution1280x960Fps12:
                        width = 1280;
                        height = 960;
                        break;
                }
            }
            else if (sensor.DepthStream.IsEnabled)
            {
                coordinateMapper = new CoordinateMapper(sensor);
                var depthPoint = coordinateMapper.MapSkeletonPointToDepthPoint(position, sensor.DepthStream.Format);

                x = depthPoint.X;
                y = depthPoint.Y;

                switch (sensor.DepthStream.Format)
                {
                    case DepthImageFormat.Resolution80x60Fps30:
                        width = 80;
                        height = 60;
                        break;
                    case DepthImageFormat.Resolution320x240Fps30:
                        width = 320;
                        height = 240;
                        break;
                    case DepthImageFormat.Resolution640x480Fps30:
                        width = 640;
                        height = 480;
                        break;
                }
            }
            else
            {
                width = 1;
                height = 1;
            }
            return new Vector2(x / width, y / height);
        }

    }

}
